#include "mpu6050_new.h"
#include "gpio_pin.h"

// MPU6050引脚定义
#define MPU6050_SDA    GPIO_PIN_21
#define MPU6050_SCL    GPIO_PIN_22

int16_t mpu6050_gyro_x = 0, mpu6050_gyro_y = 0, mpu6050_gyro_z = 0;          // 三轴陀螺仪数据      GYRO (陀螺仪)
int16_t mpu6050_acc_x = 0,  mpu6050_acc_y = 0,  mpu6050_acc_z = 0;           // 三轴加速度计数据     ACC  (accelerometer 加速度计)

int16_t mpu6050_gyro_x_e = 0,  mpu6050_gyro_y_e = 0,  mpu6050_gyro_z_e = 0;  // 三轴陀螺仪零漂误差

// MPU6050初始化
void MPU6050_Init(void)
{
    Wire.setPins(MPU6050_SDA,MPU6050_SCL);             // 设置IIC通信引脚
    Wire.begin();

	MPU6050_WriteReg(MPU6050_PWR_MGMT_1,   0x01);
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2,   0x00);
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV,   0x04);      // 采样率设置 200Hz
	MPU6050_WriteReg(MPU6050_CONFIG,       0x06);      
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG,  0x18);      // 陀螺仪    ±2000°/s
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x00);      // 加速度计  ±2g

    // 去陀螺仪零漂移
    // 有时候启动还是会有零漂，后续还得注意
    MPU6050_GyroErro();

    // 四元数初始化
    quaternion_init();
                  
    // MPU6050中断配置
    // MPU6050_WriteReg(MPU6050_INT_EN_REG, 0x01);
    // MPU6050_WriteReg(MPU6050_INTBP_CFG_REG, 0x9c);
}

// 获取MPU6050的地址
uint8_t MPU6050_GetID(void)
{
    uint8_t name = MPU6050_ReadReg(MPU6050_WHO_AM_I);

    if(name != 0x00)
    {
        Serial.printf("name = %#x\n",name);     // 16进制输出
        delay(10);
    }

	return name;
}

// 获取加速度计数据
void MPU6050_GetAcc(void)
{
    uint8_t DataH, DataL;

	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
	mpu6050_acc_x = (DataH << 8) | DataL;

    DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
	mpu6050_acc_y = (DataH << 8) | DataL;

    DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
	mpu6050_acc_z = (DataH << 8) | DataL;
}

// 获取陀螺仪计数据
void MPU6050_GetGyro(void)
{
    uint8_t DataH, DataL;

    DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
	mpu6050_gyro_x = (DataH << 8) | DataL;

	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
	mpu6050_gyro_y = (DataH << 8) | DataL;

	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
	mpu6050_gyro_z = (DataH << 8) | DataL;

    mpu6050_gyro_x -= mpu6050_gyro_x_e;
    mpu6050_gyro_y -= mpu6050_gyro_y_e;
    mpu6050_gyro_z -= mpu6050_gyro_z_e;
}

// 获取MPU6050的欧拉角
void MPU6050_GetEuler(void)
{
    quaternion_update();       
}

// 去陀螺仪零漂
void MPU6050_GyroErro(void)
{
    int16_t sum_x = 0;
    int16_t sum_y = 0;
    int16_t sum_z = 0;

    for(uint8_t i = 0;i < 200 ;i++)
    {
        MPU6050_GetGyro();

        sum_x += mpu6050_gyro_x;
        sum_y += mpu6050_gyro_y;
        sum_z += mpu6050_gyro_z;
    }

    mpu6050_gyro_x_e = sum_x / 200.0f;
    mpu6050_gyro_y_e = sum_y / 200.0f;
    mpu6050_gyro_z_e = sum_z / 200.0f;
}

uint8_t MPU6050_ReadReg(uint8_t Address)
{
	Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(Address);
    Wire.endTransmission(false);

    uint8_t a = Wire.requestFrom(MPU6050_ADDRESS,1);

    if(Wire.available())
    {
        // Serial.printf("Data\n");
        return Wire.read();
    }
    else
    {
        Serial.printf("NULL\n");
    }
    
    return 0;
}

void MPU6050_WriteReg(uint8_t Address, uint8_t Data)
{
	Wire.beginTransmission(MPU6050_ADDRESS);
    Wire.write(Address);
    Wire.write(Data);

    Wire.endTransmission(true);

}